/*
 * Utilities.cpp
 *
 *  Created on: Aug 29, 2012
 *      Author: alberto
 */

#include "Utilities.h"


using namespace cv;
using namespace std;

bool compareMatch( DMatch A, DMatch B){

	return A.distance > B.distance;
}

double calculateAngle(double x0, double y0, double x1, double y1){

	double vx = x1 - x0;
	double vy = y1 - y0;

	return atan2(vy, vx)*180/3.14159;
}

int* generateRandomIndexes(int boundary){

	assert(boundary > 0);
	int64 tc = getTickCount() % 892834928;
	//cout << "boundary: " << boundary << endl;
	RNG rng(tc);
	static int rnNumbers[3];
	int i = 0;
	unsigned int r;

	while(i<3){
		bool repeat = false;
		r = rng.operator unsigned int();
		r = r % boundary;
		for(int j=i-1; j >= 0; j--){
			if(r == rnNumbers[j]){
				repeat = true;
				break;
			}
		}
		if(!repeat){
			rnNumbers[i] = r;
			i++;
		}
	}
	return rnNumbers;
}

void calculateCumulativeDistribution(const Mat& histogram, size_t totalSize, vector<double>& cumulativeDistribution){
	double cumulative = 0;
	size_t bins = histogram.total();

	//cout << totalSize << endl;

	for(size_t i=0; i<bins; i++){
		float binValue = histogram.at<float>(i);
		double prob = binValue/totalSize;
		cumulative += prob;
		cumulativeDistribution.push_back(cumulative);
		//cout << "bin " << i << ": " << cumulative << endl;
	}


}

size_t findNearest(const vector<double>& elements, double k){
	int up_lim = elements.size() - 1;
	int bot_lim = 0;
	double smallest_value = 100000;
	int smallest_idx = -1;

	while(up_lim >= bot_lim){

		int intv = up_lim - bot_lim;
		int middle = (bot_lim + intv/2);

		//cout << "up lim: " << up_lim << endl;
		//cout << "bot lim: " << bot_lim << endl;
		//cout << "intv: " << intv << endl;
		//cout << "middle: " << middle << endl;
		double tested = elements[middle];
		double diff = k - tested;
		//cout << "tested: " << tested << endl;
		//cout << "diff: " << diff << endl;

		if(abs(diff) < smallest_value){
			smallest_value = abs(diff);
			smallest_idx = middle;
		}

		if(diff == 0) return smallest_idx;
		if(tested > k) up_lim = middle - 1;
		if(tested < k) bot_lim = middle + 1;

		//cout << endl;
	}

	return smallest_idx;
}

size_t findNearest2(double k, const vector<double>& elements, size_t bot_lim, size_t up_lim){

	double nearest_idx = 0;
	double last_value = 100000;

	for(size_t idx = bot_lim; idx < up_lim; idx++){

		//cout << endl;
		double diff = k - elements[idx];
		if(diff == 0) return nearest_idx;
		if((float)fabs(diff) < (float)fabs(last_value)){
			nearest_idx = idx;
		}

		else if((float)fabs(diff) > (float)fabs(last_value)) return nearest_idx;
		last_value = diff;

	}

	return nearest_idx;

}

void equalizeHistogram(const Mat& histogram, size_t totalSize, vector<size_t>& mapping, vector<double>& cumulativeDistribution, Mat& eqHistogram){

	assert(eqHistogram.empty());
	assert(mapping.empty());
	assert(cumulativeDistribution.empty());

	eqHistogram = Mat::zeros(histogram.rows, 1, CV_32FC1);
	size_t bins = histogram.total();
	mapping.reserve(bins);
	double maxLevel = bins-1;

	calculateCumulativeDistribution(histogram, totalSize, cumulativeDistribution);

	//for(vector<float>::iterator it=cumDist.begin(); it != cumDist.end(); ++it) cout << *it << " " << endl;
	//cout << endl;

	for(size_t i=0; i<bins; i++){
		//cout << i << " ";
		//cout << round(cumDist[i]*maxLevel) << endl;
		mapping.push_back((size_t)(round(cumulativeDistribution[i]*maxLevel)));
		//cout << "mapping i: " << mapping[i] << endl;
		eqHistogram.at<float>(mapping[i]) += histogram.at<float>(i);
	}

	return;
}

void createHistogramMatchingMap(const vector<double>& cdf_im, const vector<double>& cdf_target, vector<size_t>& esp_mapping){

	assert(cdf_im.size() == cdf_target.size());

	size_t nbins = cdf_im.size();
	size_t bot_lim = 0;
	size_t up_lim = cdf_target.size();

	size_t i = 0;
	while(cdf_target[i] == 0) i++;
	bot_lim = i;

	i = cdf_target.size()-1;
	while(cdf_target[i] == 1) i--;
	up_lim = i+2;


	for(size_t i = 0; i < nbins; i++){
		double elmt = cdf_im[i];

		size_t map = findNearest2(elmt, cdf_target, bot_lim, up_lim);
		esp_mapping.push_back(map);
		bot_lim = map;
	}

}

void histogramMatching(Mat& imageSrc, const vector<size_t>& eq_mapping, const vector<size_t>& esp_mapping, Mat& imageDst){

	imageDst = Mat::zeros(imageSrc.rows, imageSrc.cols, imageSrc.type());
	size_t imSz = imageSrc.total();

	for(size_t i = 0; i < imSz; i++){
		//cout << "i: " << i << endl;
		size_t src_value = (size_t)imageSrc.at<uchar>(i);
		//size_t eq_idx = eq_mapping[src_value];
		uchar match_value = (uchar)esp_mapping[src_value];
		imageSrc.at<uchar>(i) = (uchar)match_value;
	}

}

void equalizeColorImage(const Mat& image, Mat& eqImage){

	Mat imHSV;
	Mat eqHSV;
	vector<Mat> channelsImage;
	vector<Mat> channelsEq;


	cvtColor(image, imHSV, CV_BGR2HSV);
	split(imHSV, channelsImage);

	channelsEq.push_back(Mat());
	channelsImage[0].copyTo(channelsEq[0]);
	channelsEq.push_back(Mat());
	channelsImage[1].copyTo(channelsEq[1]);
	channelsEq.push_back(Mat());
	equalizeHist(channelsImage[2], channelsEq[2]);
	merge(channelsEq, eqHSV);

	cvtColor(eqHSV, eqImage, CV_HSV2BGR);
}

double correlationCoefficient(const Mat& A, const Mat& B){

	Mat cvtA;
	Mat cvtB;

	A.convertTo(cvtA, 6);
	B.convertTo(cvtB, 6);

	Scalar temp;
	temp = mean(cvtA);
	double meanA = temp[0];
	temp = mean(cvtB);
	double meanB = temp[0];

	Mat auxA = cvtA - meanA;
	Mat auxB = cvtB - meanB;

	Mat prodAB = auxA.mul(auxB);

	Scalar numerator = sum(prodAB);
	Scalar denominator = sum(auxA.mul(auxA)) * sum(auxB.mul(auxB));

	double r =  numerator[0]/(sqrt(denominator[0]) + 0.001);

	//cout << "mean 1: " << meanA << endl << endl;
	//cout << "mean 2: " << meanB << endl << endl;
	//cout << "A - mean1: " << endl << auxA << endl << endl;
	//cout << "B - mean2: " << endl << auxB << endl << endl;
	//cout << "M3: " << endl << prodAB << endl << endl;
	//cout << "den: " << denominator[0] << endl << endl;
	//cout << "num: " << numerator[0] << endl << endl;
	//cout << "r: " << r << endl;

	return r;
}

double meanSqrdError(const Mat& A, const Mat& B){

	//imshow("tA", A);
	//imshow("B", B);
	//waitKey(0);
	Mat cvtA;
	Mat cvtB;

	A.convertTo(cvtA, 6);
	B.convertTo(cvtB, 6);

	double sz = (double)A.rows*A.cols;

	Mat diff = cvtA - cvtB;

	cv::pow(diff, 2, diff);

	Scalar sm = sum(diff);
	double r = sm[0];
	return r;
}

bool cmpPairSmallest(const pair<int, float> a, const pair<int, float> b){
	return a.second < b.second;
}

bool cmpPairBiggest(const pair<int, float> a, const pair<int, float> b){
	return a.second > b.second;
}


void jpegRecompression(const Mat &OriginalIm, string estName, Mat &recompIm){


    // Setting up jpeg compress info
    struct jpeg_compress_struct comp_info;
    FILE* outfile = fopen("out.jpg", "wb");

    struct jpeg_error_mgr jerr_c;
    comp_info.err = jpeg_std_error(&jerr_c);

    jpeg_create_compress(&comp_info);
    jpeg_stdio_dest(&comp_info, outfile);

    // *************


    // *************
    //cout << "[Setting up basic jpeg decompress info]" << endl;
    struct jpeg_decompress_struct dec_info;
    FILE* infile = fopen(estName.c_str(), "rb");

    struct jpeg_error_mgr jerr_d;

    dec_info.err = jpeg_std_error(&jerr_d);
    jpeg_create_decompress(&dec_info);

    jpeg_stdio_src(&dec_info, infile);
    jpeg_read_header(&dec_info, TRUE);

    // *************

    // Setting up cinfo settings according to dinfo settings
    comp_info.image_width = OriginalIm.cols;
    comp_info.image_height = OriginalIm.rows;
    comp_info.input_components = 3;
    comp_info.in_color_space = dec_info.out_color_space;
    //  *************

    // Setting up quantization tables
    jpeg_set_defaults(&comp_info);
    comp_info.quant_tbl_ptrs[0] = dec_info.quant_tbl_ptrs[0];
    comp_info.quant_tbl_ptrs[1] = dec_info.quant_tbl_ptrs[1];
    comp_info.quant_tbl_ptrs[2] = dec_info.quant_tbl_ptrs[2];
    comp_info.quant_tbl_ptrs[3] = dec_info.quant_tbl_ptrs[3];
    // *************

    // Starting compression
    jpeg_start_compress(&comp_info, true);

    JSAMPROW row_pointer;

    while(comp_info.next_scanline < comp_info.image_height)
    {
    	row_pointer = (JSAMPROW) &OriginalIm.data[comp_info.next_scanline*OriginalIm.step];
    	jpeg_write_scanlines(&comp_info, &row_pointer, 1);
    }

    jpeg_finish_compress(&comp_info);


    jpeg_destroy_compress(&comp_info);
    jpeg_destroy_decompress(&dec_info);

    fclose(infile);
    fclose(outfile);

    recompIm = imread("out.jpg");
    unlink("out.jpg");

    return;
}

template <class itr>
void printVector(string name, itr bg, itr ed){

	cout << "Printing content of vector: " << name << endl;

	cout << "[";
	for(itr it = bg; it != ed; ++it){
		cout << *it << " ";
	}
	cout << "]" << endl;

}

void matchColor(const Mat &src, const Mat &target, Mat &result){

	Mat cpSrc;
	Mat cpTgt;

	src.convertTo(cpSrc, 6);
	target.convertTo(cpTgt, 6);

	vector<Mat> chansSrc;
	split(cpSrc, chansSrc);
	Mat meansSrc, meansTarget;
	Mat stdevsSrc, stdevsTarget;
	meanStdDev(cpSrc, meansSrc, stdevsSrc);

	meanStdDev(cpTgt, meansTarget, stdevsTarget);
	//cout << "3" << endl;
	for(size_t c = 0; c<3; c++){

		chansSrc[c] = chansSrc[c] - meansSrc.at<double>(c);

		chansSrc[c] = chansSrc[c]/stdevsSrc.at<double>(c);

		chansSrc[c] = chansSrc[c]*stdevsTarget.at<double>(c);

		chansSrc[c] = chansSrc[c] + meansTarget.at<double>(c);

	}
	//cout << "4" << endl;
	merge(chansSrc, result);
	result.convertTo(result, 16);
	//cout << "5" << endl;
	/*imshow("Original", src);
	imshow("Alvo", target);
	imshow("resultado", result);
	waitKey(0);
	destroyAllWindows();*/

	return;

}



